#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdarg.h>
#include "GA.h"
#include "GARand.h"

#ifdef GD
#include "main.h"
#include "systick.h"
#endif

typedef struct
{
    int idv[LENGTH];
    int fitValue;
} Idv; // 种群individual

const struct GATargetFunc *target;

static Idv pop[POP_SIZE];
static int row[ROW_LENGTH];
static int indv[LENGTH];
static double cumsump[(int)(RETENTION_RATION * POP_SIZE)];

int best[MAX_GENERATION] = {0};
int idvBest[LENGTH] = {0};
unsigned char ris[NY]; // 需要传递回去

// static int rdtest = 0;

// #ifdef GD
// extern int get_Feedback();
// extern void Transmit_data(unsigned char dat[]);
// extern void ga_print(char *str, ...);
// #endif

unsigned char popToRisROW(int *x);
unsigned char *popToRis(int *x);
int *risToPopROW(unsigned char rr);
int *risToPop(unsigned char *rr);
void initialPop(unsigned char *);
int targetFuncCheck(void);
void fitnessFun(int, int);
void popSort(void);
void popRemain(void);
void mutation(int);
int selection(double *cumsump);
void crossOver(int, int, int);
void crossOver2(int, int, int, int);
void prt(char *s, ...);

#ifdef GD
    #define  GAPrint(fmt, arg...)  target->print(fmt, ##arg) 
#else
    #define  GAPrint(fmt, arg...)  printf(fmt, ##arg) 
    // 可变参数部分为空时，printf(fmt,)
    // ##的作用就是在这种情况下消除多余的 ','
#endif

/** debug用 种群个体打印
 *   输入：打印个体 [start, end)
 */
void printGA(int start, int end)
{
    for (int i = start; i < end; ++i)
    {
        long int sum = 0;
        GAPrint("pop[%d]: ", i);
        for (int j = 0; j < LENGTH; ++j)
        {
            sum = sum << 1;
            sum += pop[i].idv[j];
            GAPrint("%d, ", pop[i].idv[j]);
        }
        GAPrint(" sum: %x\r\n", sum);
    }
}

/**  _ _，启动！
 *   输入 可选 输入一个ris状态加入初始种群
 *   输出：最终ris状态
 */
unsigned char *GA(unsigned char *risState)
{
    int generation = 0;
    int start = 0, end = POP_SIZE; // [start, end)
    int count = 0;
    int th = RETENTION_RATION * POP_SIZE; // [0, th)
#ifdef GD
    while (targetFuncCheck() != 0)
    {
        GAPrint("targetFuncCheck failed\r\n");
    }
    GAPrint("targetFunc Register\r\n");
#endif

    GARandInit();
    // // test rand
    // for (int i = 0; i < 10; ++i)
    // {
    //     rdtest = GARand(100);
    //     GAPrint("rdtest%d:%d\r\n", i, rdtest);
    // }

    GAPrint("_ _,start!\r\n");
    initialPop(risState);
    // printGA(0, POP_SIZE);

    while (generation < MAX_GENERATION && count < DIFF_COUNT)
    {
        fitnessFun(start, end); // [start, end)
        // start = th;             // 保留的个体的适应度是否重复计算

        popSort();

        best[generation] = pop[0].fitValue;                 // 每次迭代的最大适应度
        memcpy(idvBest, &pop[0].idv, LENGTH * sizeof(int)); // 迭代的最优个体
        printGA(0, 1);

        if (generation > 1 && best[generation] - best[generation - 1] < DIFF)
        {
            ++count;
        }

        popRemain(); // 按比例保留种群个体，并计算保留个体的累计概率

        for (int i = th; i < POP_SIZE; i++)
        { // 一次产生1个子代
            // 在保留的个体中选择两个产生子代的父代
            int seln1 = selection(cumsump);
            int seln2 = seln1;
            while (seln1 == seln2)
            {
                seln2 = selection(cumsump);
            }

            crossOver(seln1, seln2, i); // 父代交叉产生1个子代

            mutation(i); // 子代的变异；
        }

        // for (int i = th; i < POP_SIZE - 1; i += 2) { // 一次产生两个子代
        //     // 在保留的个体中选择两个产生子代的父代
        //     int seln1 = selection(cumsump);
        //     int seln2 = selection(cumsump);
        //     crossOver2(seln1, seln2, i, i + 1);  // 父代交叉产生两个子代
        //     mutation(i);        // 子代的变异；
        //     mutation(i + 1);
        // }

        // printGA(0, POP_SIZE);
        // target->delay(2000);

        GAPrint("generation: %d, %d\r\n", generation, best[generation]);

        generation++;
    }

    GAPrint("end\r\n");

    // unsigned char* best = popToRis(idvBest);
    return popToRis(idvBest);
}

/** 种群个体的一列转化为ris状态的一列
 *   输入：int x[ROW_LENGTH] 只处理ROW_LENGTH长度,可以更长
 *   输出：unsigned char rr
 */
unsigned char popToRisROW(int *x)
{
    unsigned char ris0 = 0;
    // eg. 10011001 -> 0x55
    for (int i = 0; i < ROW_LENGTH; ++i)
    {
        ris0 = (ris0 << 1) + x[i];
    }
    return ris0;
}

/** 种群转化为ris状态
 *   输入：int x[LENGTH]
 *   输出：unsigned char ris[Ny]
 */
// unsigned char ris[NY];
unsigned char *popToRis(int *x)
{
    for (int i = 0; i < NY; ++i)
    {
        // 当前列的起始地址： x起始地址 + (i*ROW_LENGTH)
        ris[i] = popToRisROW(x + (i * ROW_LENGTH));
    }
    return ris;
}

/** 一列ris状态转化为种群个体的一列
 *   输入：unsigned char rr
 *   输出：int row[ROW_LENGTH]
 */
// int row[ROW_LENGTH];
int *risToPopROW(unsigned char rr)
{
    // eg. 0x55 -> 10011001
    for (int i = ROW_LENGTH - 1; i >= 0; --i)
    {
        // 取rr的最低位作为row的最后一个元素
        row[i] = rr & 1;
        rr = rr >> 1; // 右移至下一位
    }
    return row;
}

/** ris状态转化为种群个体
 *   输入：unsigned char rr[NY]
 *   输出：int inv[Length]
 */
// int indv[LENGTH];
int *risToPop(unsigned char *rr)
{
    for (int i = 0; i < NY; ++i)
    {
        int *ptRow = risToPopROW(rr[i]);
        for (int j = i * ROW_LENGTH, k = 0; k < ROW_LENGTH; ++j, ++k)
        {
            indv[j] = ptRow[k];
        }
    }
    return indv;
}

/** 种群初始化
 *   根据宏定义使用码本初始化，其余个体使用随机初始化
 *   操作全局变量
 */
void initialPop(unsigned char *risState)
{
    /*
     *    码本生成 未做
     */
    int codeBookCnt = DFT * NY * NZ - A_A * P_A;

    // 将输入的ris状态加入初始种群
    if (risState != NULL)
    {
        int *pop0 = risToPop(risState);
        // 复制到的码本个体之后的第一个
        memcpy(&pop[codeBookCnt].idv, pop0, LENGTH * sizeof(int));
        codeBookCnt++;
    }

    // 随机初始化其余种群
    for (int i = codeBookCnt; i < POP_SIZE; ++i)
    {
        for (int j = 0; j < NY; j++)
        {
            int r = GARand(RAND_RANGE); // 随机生成一列ris状态

            // 将生成的一列ris状态转成pop数组，复制到pop的个体的idv对应位置
            int *ptrRow = risToPopROW(r);
            memcpy((int *)(&pop[i].idv) + j * ROW_LENGTH, ptrRow, ROW_LENGTH * sizeof(int));
            // &pop[i].idv + j * ROW_LENGTH 并不是想要的结构体内数组的第n个元素的地址
            // 转换成 (int *) 才能正确的使用指针 ++
        }
    }
}

/** 注册GA需要的适应度计算回调函数
 *   输入包含函数指针的结构体
 *   成功返回0
 */
int GATargetFuncRegister(const struct GATargetFunc *drv)
{
    if (!drv)
    {
        return -1;
    }
    if (!drv->get || !drv->delay || !drv->send)
    {
        return -2;
    }
    target = drv;
    return 0;
}

/** 检查GA需要的适应度计算函数是否已注册
 *   成功返回0
 */
int targetFuncCheck()
{
    if (!target)
    {
        return -1;
    }
    if (!target->get || !target->delay || !target->send || !target->print)
    {
        return -2;
    }
    return 0;
}

/** PC下测试用的适应度计算
 *   输入ris状态
 *   返回目标值大小
 */
int targetTest(unsigned char *x)
{
    // return GARand(100);

    int sum = 0;
    for (int i = 0; i < NY; ++i)
    {
        sum += x[i];
    }
    return sum;
}

/** 获取ris状态的适应度
 *   调用ris状态发送函数，并等待READ_DELAY时间后读取信号数据作为适应度
 *   输入ris状态
 *   返回目标值大小
 */
int targetFun(unsigned char *x)
{
        // Fitvalue = calcSinrG(x);
        // Fitvalue = calcPG(x);
#ifdef GD
    while (*(target->stop))
    {
        // GAPrint("GA stop\r\n");
        target->delay(10);
    }
    target->send(x);
    target->delay(READ_DELAY);

    int feedback = target->get();
    GAPrint("feedback:%d\r\n", feedback);
    return feedback;
#else
    // just for test
    return targetTest(x);
#endif
}

/** 计算种群适应度
 *   输入为待计算的种群的下标起始、结束[start,end)
 *   适应度结果存于全局变量中
 */
void fitnessFun(int start, int end)
{
    // int popsize = end - start; //[0, POP_SIZE) [start,end)
    for (int i = start; i < end; ++i)
    {
        // 取种群的第i个个体，转换为RIS状态，求其适应度
        pop[i].fitValue = targetFun(popToRis(pop[i].idv));
    }
}

/*  排序比较函数cmp：
 *   返回类型必须是int;
 *   两个参数的类型必须都是const void *;
 *   如果是升序,那么就是如果a比b大返回一个正值,小则负值,相等返回0;
 */
int cmp(const void *a, const void *b)
{
    // a是个void *类型，要先用(Idv*）将它转成Idv*类型，然后再用*取值，变成Idv类型，才能比较大小。
    Idv A = *(Idv *)a;
    Idv B = *(Idv *)b;
    // 降序排列
    if (A.fitValue < B.fitValue)
    {
        return 1;
    }
    else if (A.fitValue == B.fitValue)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

/*   Idv的快排：
 *    降序
 */
void swap(Idv *arr, int x, int y)
{
    if (x == y)
    {
        return;
    }
    int size = sizeof(pop[0]) * 1;
    Idv tmp;
    memcpy(&tmp, &pop[x], size);
    memcpy(&pop[x], &pop[y], size);
    memcpy(&pop[y], &tmp, size);
}

int Partition(Idv *arr, int low, int high)
{
    // low mid end 中取中间值作为基准
    int mid = low + (high - low) / 2; // 中间元素的下标
    int max = high, min = low;
    int second;
    if (arr[low].fitValue > arr[high].fitValue)
    {
        max = low;
        min = high;
    }
    if (arr[mid].fitValue > arr[max].fitValue)
    {
        second = max;
    }
    else if (arr[mid].fitValue > arr[min].fitValue)
    {
        second = mid;
    }
    else
    {
        second = min;
    }
    if (second != low)
    {
        swap(arr, second, low);
    }

    int pivotkey = arr[low].fitValue; // pivotkey取数组第一个元素
    while (low < high)
    { // low和high相等的时候就说明low和high同时指向了基准的下标
        while (low < high && arr[high].fitValue <= pivotkey)
        {
            // 从数组右边向左边开始找有没有比基准大的，有就跳出循环，并和基准左边的数交换，确保大的数都在基准左边。
            high--;
        }
        swap(arr, low, high);
        while (low < high && arr[low].fitValue >= pivotkey)
        {
            // 从数组左边向右边开始找有没有比基准小的，有就跳出循环，并和基准右边的数交换，确保小的数都在基准右边。
            low++;
        }
        swap(arr, low, high);
    }
    return low; // 返回基准下标，low和high都可以
}

void QSort(Idv arr[], int low, int high)
{
    int pivot; // 基准的下标
    if (low < high)
    {
        pivot = Partition(arr, low, high); // 获取基准下标，目的是为了把数组分成左右两部分
        QSort(arr, low, pivot - 1);        // 把左边的部分继续排序
        QSort(arr, pivot + 1, high);       // 把右边的部分继续排序
    }
}

/** 种群排序
 *   根据适应度对种群进行排序，会交换结构体数组元素位置
 *   使用Quicksort
 *   操作全局变量
 */
void popSort()
{
#ifdef USE_MYQSORT
    // 降序 快排 pivot优化
    QSort(pop, 0, POP_SIZE);
#else
    // 降序 快排 库函数
    qsort(pop, POP_SIZE, sizeof(pop[0]), cmp);
#endif
}

/** 选出种群中要保留的个体
 *   种群已排序，保留前rr的个体 计算累计概率
 *   操作全局变量
 */
// double cumsump[(int)(RETENTION_RATION * POP_SIZE)];
void popRemain()
{
    // 保留的门限下标
    int th = RETENTION_RATION * POP_SIZE;
    // [0, th)
    double fit[th];

    // 差值处理
    if (K < 0)
    { // 指数操作
        for (int i = 0; i < th; ++i)
        {
            if (pop[i].fitValue + (-K) > pop[th - 1].fitValue)
            {
                fit[i] = exp(pop[i].fitValue - pop[th - 1].fitValue + (-K));
            }
            else
            {
                fit[i] = exp(1);
            }
        }
    }
    else
    { // 直接＋
        for (int i = 0; i < th; ++i)
        {
            fit[i] = K + pop[i].fitValue - pop[th - 1].fitValue;
        }
    }

    // 求保留的个体的适应度差值的和
    double sum = 0;
    for (int i = 0; i < th; ++i)
    {
        sum += fit[i];
    }

    // 归一化
    for (int i = 0; i < th; ++i)
    {
        fit[i] /= sum;
    }

    // 求累计概率
    cumsump[1] = fit[1];
    for (int i = 0; i < th; ++i)
    {
        cumsump[i] = cumsump[i - 1] + fit[i];
    }
}

/** 变异或交叉的抽奖函数
 *   输入概率
 *   返回是否变异或交叉
 */
static int ifCroIfMut(double p)
{
    int r = GARand(100);
    // GAPrint("ifCroIfMut: %d : %d\r\n", 2, r, (int)(p * 100));
    return r < (p * 100);
}

/** 按概率持续变异
 *   输入要进行变异操作的个体的下标
 *   操作全局变量
 */
void mutation(int seln)
{
    // 按概率持续变异
    while (ifCroIfMut(P_MUTATION))
    {
        // 抽变异的染色体位置
        int position = GARand(LENGTH); // 对应位置0-1翻转

        // printGA(seln, seln + 1);

        pop[seln].idv[position] = (pop[seln].idv[position] + 1) % 2;

        // printGA(seln, seln + 1);
        // GAPrint("mutation: %d\r\n", 1, seln);
    }
    pop[seln].fitValue = 0; // 新个体的适应度初始化为0
}

/** 新种群选择操作：
 *   根据输入的累计概率从种群选择1个个体
 *   输入：种群累计概率
 *   返回：被选中的个体序号
 */
int selection(double *cumsump)
{
    double r = GARand(RAND_MAX) / (double)(RAND_MAX); // 随机浮点数[0,1)
    int seln = 0;
    while (cumsump[seln] < r)
    {
        // 第一个大于rand的区间为被选中的染色体区间。
        ++seln;
    }
    return seln;
}

/** 新种群交叉函数
 *   节点前后分段  分段互换
 *   输入为：选中的两个个体的序号 子代的序号
 *   对全局变量进行操作
 */
void crossOver(int seln1, int seln2, int offspring)
{
    if (P_CROSSOVER == 0)
    {
        // 随机选取一个交叉节点
        int position = 1 + GARand(LENGTH); // [1, LENGTH]
        // [0,position) [position, LENGTH)

        // 第一个子代  取seln1的position前序列，seln2的position后序列
        memcpy(&pop[offspring].idv, &pop[seln1].idv, position * sizeof(int));
        memcpy((int *)(&pop[offspring].idv) + position, (int *)(&pop[seln2].idv) + position, (LENGTH - position) * sizeof(int));
    }
    else
    {
        // 按概率持续交叉
        memcpy(&pop[offspring].idv, &pop[seln1].idv, LENGTH * sizeof(int));

        while (ifCroIfMut(P_CROSSOVER)) // 按概率持续交叉
        {
            // 抽交换的染色体位置 一次两个(一个ris单元)
            int position = 2 * GARand(LENGTH / 2); // 2 * [0, LENGTH / 2 - 1]
            // [2*p] 和 [2*p+1]

            // printGA(offspring, offspring + 1);
            // printGA(seln2, seln2 + 1);

            memcpy((int *)(&pop[offspring].idv) + position, (int *)(&pop[seln2].idv) + position, 2 * sizeof(int)); // 第一个子代

            // printGA(offspring, offspring + 1);
        }
        // GAPrint("new: %d\r\n", offspring);
    }
}

/** 新种群交叉函数
 *   节点前后分段  分段互换
 *   输入为：选中的两个个体的序号 子代的序号
 *   对全局变量进行操作
 */
void crossOver2(int seln1, int seln2, int offspring1, int offspring2)
{
    if (P_CROSSOVER == 0)
    {
        // 随机选取一个交叉节点
        int position = 1 + GARand(LENGTH); // [1, LENGTH]
        // [0,position) [position, LENGTH)

        // 第一个子代  取seln1的position前序列，seln2的position后序列
        memcpy(&pop[offspring1].idv, &pop[seln1].idv, position * sizeof(int));
        memcpy((int *)(&pop[offspring1].idv) + position, (int *)(&pop[seln2].idv) + position, (LENGTH - position) * sizeof(int));
        // 第二个子代
        memcpy(&pop[offspring2].idv, &pop[seln2].idv, position * sizeof(int));
        memcpy((int *)(&pop[offspring2].idv) + position, (int *)(&pop[seln1].idv) + position, (LENGTH - position) * sizeof(int));
    }
    else
    {
        // 按概率持续交叉
        memcpy(&pop[offspring1].idv, &pop[seln1].idv, LENGTH * sizeof(int));
        memcpy(&pop[offspring2].idv, &pop[seln2].idv, LENGTH * sizeof(int));

        while (ifCroIfMut(P_CROSSOVER)) // 按概率持续交叉
        {
            // 抽交换的染色体位置 一次两个(一个ris单元)
            int position = 2 * GARand(LENGTH / 2); // 2 * [0, LENGTH / 2 - 1]
            // [2*p] 和 [2*p+1]
            memcpy((int *)(&pop[offspring1].idv) + position, (int *)(&pop[seln2].idv) + position, 2 * sizeof(int)); // 第一个子代
            memcpy((int *)(&pop[offspring2].idv) + position, (int *)(&pop[seln1].idv) + position, 2 * sizeof(int)); // 第二个子代
        }
    }
}

// void crossOver(int seln1, int seln2){
//     if (P_CROSSOVER == 0) {
//         //随机选取一个交叉节点
//         int position = 1 + rand() % LENGTH;    // [1, LENGTH]
//         // 取seln(1)的position前序列，seln(2)的position前序列交叉
//         // [0,position)
//         int tmp[position];
//         memcpy(tmp, pop[seln1].idv, position);
//         memcpy(pop[seln1].idv, pop[seln2].idv, position);
//         memcpy(pop[seln2].idv, tmp, position);
//     } else {
//         // 按概率持续交叉
//         while (ifCroIfMut(P_CROSSOVER)) {
//             // 抽交换的染色体位置 一次两个(一个ris单元)
//             int position = 2 * rand() % (LENGTH / 2); // 2 * [0, LENGTH / 2 - 1]
//             // 交换 [2*p] 和 [2*p+1]
//             int tmp[2];
//             memcpy(tmp, pop[seln1].idv + position, 2);
//             memcpy(pop[seln1].idv + position, pop[seln2].idv + position, 2);
//             memcpy(pop[seln2].idv + position, tmp, 2);
//         }
//     }
// }

/* 通用数字打印函数可以把整型值打印成 10进制数,8进制数,2进制数,16进制数
 * 参数: 1.需要打印的整数,无符号长整型是为了兼容地址格式打印; 2.打印的进制
 */
void printNum(unsigned long num, int base)
{
    /* 递归结束条件 */
    if (num == 0)
    {
        return;
    }

    /* 继续递归 */
    printNum(num / base, base);

    /* 逆序打印结果 */
    putchar("0123456789abcdef"[num % base]);
}

/* 打印十进制数
 * 参数: 十进制整数
 * 返回值: 无
 */
void printDeci(int dec)
{
    /* 处理有符号整数为负数时的情况 */
    if (dec < 0)
    {
        putchar('-');
        dec = -dec; // 该操作存在溢出风险:最小的负数没有对应的正数
    }

    /* 处理整数为时0的情况 */
    if (dec == 0)
    {
        putchar('0');
        return;
    }
    else
    {
        printNum(dec, 10); // 打印十进制数
    }
}

/* 打印八进制整数
 * 参数: 无符号整数
 * 返回值: 无
 */
void printOct(unsigned oct)
{
    if (oct == 0) // 处理整数为0的情况
    {
        putchar('0');
        return;
    }
    else
    {
        printNum(oct, 8); // 打印8进制数
    }
}

/* 打印十六进制整数
 * 参数: 无符号整数
 * 返回值: 无
 */
void printHex(unsigned hex)
{
    if (hex == 0) // 处理整数为0的情况
    {
        putchar('0');
        return;
    }
    else
    {
        printNum(hex, 16); // 打印十六进制数
    }
}

/* 打印地址
 * 参数: 待打印的地址
 * 返回值: 无
 */
void printAddr(unsigned long addr)
{
    /* 打印前导"0x" */
    putchar('0');
    putchar('x');

    /* 打印地址:格式和十六进制一样 */
    printNum(addr, 16);
}

/* 打印字符串
 * 参数: 字符串地址
 * 返回值: 无
 */
void printStr(char *str)
{
    int i = 0;

    while (str[i] != '\0')
    {
        putchar(str[i++]);
    }
}

/* 打印浮点数
 * 参数: 待打印浮点数
 * 返回值: 无
 */
void printFloat(double f)
{
    int temp;

    /* 先打印整数部分 */
    temp = (int)f;
    printNum(temp, 10);

    /* 分隔点 */
    putchar('.');

    /* 打印小数部分 */
    f -= temp;
    if (f == 0)
    {
        /* 浮点型数据至少六位精度 */
        for (temp = 0; temp < 6; temp++)
        {
            putchar('0');
        }
        return;
    }
    else
    {
        temp = (int)(f * 1000000);
        printNum(temp, 10);
    }
}

void prt(char *s, ...)
{
    int i = 0;

    /* 可变参 */
    va_list va_ptr;
    va_start(va_ptr, s);

    /* 循环打印所有格式字符串 */
    while (s[i] != '\0')
    {
        /* 普通字符正常打印 */
        if (s[i] != '%')
        {
            putchar(s[i++]);
            continue;
        }

        /* 格式字符特殊处理 */
        switch (s[++i]) // i先++是为了取'%'后面的格式字符
        {               /* 根据格式字符的不同来调用不同的函数 */
        case 'd':
            printDeci(va_arg(va_ptr, int));
            break;
        case 'o':
            printOct(va_arg(va_ptr, unsigned int));
            break;
        case 'x':
            printHex(va_arg(va_ptr, unsigned int));
            break;
        case 'c':
            putchar(va_arg(va_ptr, int));
            break;
        case 'p':
            printAddr(va_arg(va_ptr, unsigned long));
            break;
        case 'f':
            printFloat(va_arg(va_ptr, double));
            break;
        case 's':
            printStr(va_arg(va_ptr, char *));
            break;
        default:
            break;
        }

        i++; // 下一个字符
    }

    va_end(va_ptr);
}

// void GAPrint(const char *str, ...)
// {
//     va_list args;
//     va_start(args, str);
// #ifdef GD
//     // printf(str, args);
//     target->print(str, args);
// #else
//     printf(str, args);
// #endif
//     va_end(args);
// }